/**
 * @file:          StepperMotorController.h
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.06.07
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.06.07,18:20:05
 */
#ifndef _STEPPERMOTORCONTROLLER_H_
#define _STEPPERMOTORCONTROLLER_H_

/* Include Files **************************************************************/
#include "Datatype.h"
#include "ModuleList.h"
#include "DriverList.h"
#include "base_math.h"
#include "Protection.h"

/* Macro Definition ****************************************************/
#define CMD_TABLE_NUM_SIZE 30

#define BOOT_SKIP_KEY ((uint16_t)0x4286)
#define BOOT_ONCE_KEY ((uint16_t)0x8567)

#define UNIT_RAD
#define UNIT_DEGREE
#define UNIT_REVOLUTION

#define UNIT_RAD_PER_SECOND
#define UNIT_RPM
#define UNIT_REVOLUTION_PER_SECOND

#define RPM2RAD (BM_2PI / 60)

#define BUFFER_SPEED 10

#define WAIT_UNTIL_TIME_SECTION(stepper_motor_controller, n)    \
    while ((stepper_motor_controller)->timer_section_num < (n)) \
    {                                                           \
    }

/* Data Type Definition *******************************************************/
typedef struct CmdUnit
{
    char id;
    char broadcast_support;
    char *name;
    char *description;
    char *usage;
    void (*func)(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
} CmdUnit;

typedef struct VerionPack4
{
    union
    {
        uint32_t all;
        struct
        {
            uint8_t compiled;
            uint8_t patch;
            uint8_t minor;
            uint8_t major;
        } byte;
    } number;
    char *release_date;
}VerionPack4;

typedef struct VerionPack2
{
    union
    {
        uint16_t all;
        struct
        {
            uint8_t minor;
            uint8_t major;
        } byte;
    } number;
    char *release_date;
}VerionPack2;

typedef struct StepperMotorController
{
    /* public data */
    StepperMotor *stepper_motor;
    Controller *controller;
    DriverBoard *driver_board;
    Encoder *encoder;
    Mcu *mcu;
    FlashManager *flash_manager_param;
    // FlashManager *flash_manager_encoder_table;
    Logger *logger;

    Protection protection;
    // SCurve scurve;
    CommunicationInterface communication_interface;
    Chopper chopper;
    StepController step_controller;
    MotorRippleTableCalibrator motor_ripple_table_calibrator;

    /* param */
    VerionPack4 param_firmware_version;

    uint16 param_boot_key;
    uint16_t param_system_startup_delay_ms;
    float param_pwm_period_s;
    float param_pos_control_period_s;
    float param_cur_control_period_s;
    float param_task_period_s;
    uint16 param_use_default_param;
    uint16 param_singleturn_control_mode;
    uint16 param_flash_key_write_retries_num;

    enum
    {
        Emergency_Pin_Mode_Null = 0,
        Emergency_Pin_Mode_Stall,
        Emergency_Pin_Mode_ProtectionSignal,
        Emergency_Pin_Mode_CutterStall,
        Emergency_Pin_Mode_Test
    };
    uint16_t param_emergency_pin_mode;
    enum
    {
        StallMode_Null = 0,
        StallMode_Cur,
        StallMode_Pos
    };
    uint16_t param_stall_mode;
    float32 param_stall_cur_A;
    float32 param_stall_pos_err_rad;
    float32 param_stall_pos_err_rad_for_slicer;
    int32 param_mech_offset;
    float param_elec_offset;
    int16 param_phase_order_invert;

    /* dir of elec offset calibration and encoder_calibtate_official */
    int16 param_calibration_dir;

    /* elec offset calibration param */
    int32 param_encoder_cal_count_max;
    uint16 param_encoder_read_count;
    uint16 param_delay_ms_per_step;
    uint16 param_delay_ms_per_read;
    float32 param_ud_cal_set;
    float32 param_motion_dir;

    /* encoder_calibtate_official param */
    uint32 param_encoder_calibtate_official_cal_count_max;
    float32 param_encoder_calibtate_official_ud_cal_set;
    uint32 param_encoder_calibration_round_count;
    float param_elec_angle_calibration_standar_deg;

    int32 in_encoder_cmd;
    float32 in_pos_cmd;
    float32 in_spd_cmd;
    float32 in_acc_cmd;
    float32 in_ualpha_cmd;
    float32 in_ubeta_cmd;

    uint16 encoder_calibration_en;
    uint16 elec_zero_offset_calibration_en;
    uint16 n_multiple_of_timmer_tick;
    uint32 timer_tick;
    uint16 timer_section_num;
    uint16 run_start;
    int32 isr_run_num;
    uint32 isr_tick;
    uint16 encoder_calibration_official_procedure_num;
    uint32 encoder_calibration_official_count_max;
    uint32 encoder_calibration_official_count;
    float encoder_calibration_official_pos_addition;
    float encoder_calibration_official_pos_cmd;
    uint16_t init_procedure_num;
    float elec_offset_err_deg;
    uint16_t stall_is_trigger;

    uint16 init_done;
    union
    {
        uint16 u16data;
        struct
        {
            uint16 mcu : 1;
            uint16 encoder : 1;
            uint16 driver_board : 1;
            uint16 stepper_motor : 1;
            uint16 controller : 1;
            uint16 communication_interface : 1;
            uint16 adc_calibration : 1;
            uint16 encoder_calibration : 1;
            uint16 scurve : 1;
            uint16 motor_phase_check_pass : 1;
            uint16 reserve : 7;
        } bit;
    } init_flag;

    union
    {
        uint16_t u16data;
        struct
        {
            uint16_t system_identification_RL : 1;
            uint16_t elec_offset_checked : 1;
            uint16_t phase_order_checked : 1;
            uint16_t encoder_calibration_official_checked : 1;
            uint16_t flash_param_valid_checked : 1;
            uint16_t reserve : 11;
        } bit;
    } self_check_flag;

    int32 encoder_fdb;
    int32 encoder_fdb_last;
    int32 encoder_fdb_err;
    int32 encoder_fdb_abs;
    float32 pos_fdb_encoder;
    float32 pos_fdb_abs;
    float32 elec_angle_fdb;
    float32 spd_fdb;
    float32 spd_fdb_encoder;
    uint16 encoder_buff_index;
    int32 encoder_buff[BUFFER_SPEED];
    float32 ialpha_fdb_raw;
    float32 ibeta_fdb_raw;
    float32 id_fdb;
    float32 iq_fdb;
    int32 encoder_fdb_tmp;
    float foc_theta_tmp;
    int32 encoder_fdb_tim;
    float foc_theta_tim;
    float elec_period_pulse;
    float encoder_2_elec_angle;
    float encoder_2_pos_rad;
    float pos_rad_2_encoder;
    float encoder_2_spd_radps;

    /* test */
    uint16_t test_sample_trig;

    int32 cmd_pulse;

    /* elec_angle calibration info */
    struct
    {
        int32_t encoder_move_max;
        int32_t encoder_move_min;
        int32_t encoder_move_err;
        int16_t encoder_moved;

        float elec_angle_offset_max_deg;
        float elec_angle_offset_min_deg;
        float elec_angle_offset_err_deg;
        float elec_angle_offset_average_deg;
        float elec_angle_offset_max_pulse;
        float elec_angle_offset_min_pulse;
        float elec_angle_offset_err_pulse;
        float elec_angle_offset_average_pulse;
        uint16_t elec_angle_offset_err_pass;
    } elec_angle_calibration_info;

    /* cmd system related */
    CmdUnit cmd_table[CMD_TABLE_NUM_SIZE];
    uint16_t cmd_nums;
    uint16_t cmd_help_task_trigger;
    uint16_t cmd_help_task_index;
    uint16_t cmd_helpcmd_task_trigger;
    uint16_t cmd_helpcmd_task_cur_index;

    /* result output format */
    enum
    {
        ResultOutputFormat_NoOutput = 0,
        ResultOutputFormat_Str,
        ResultOutputFormat_Hex
    };

    /* system identification relative */
    struct SystemId_RL
    {
        float param_Ts;
        float param_uin_series[5]; /* unit: duty */
        uint16_t param_current_buff_size;
        float param_current_stable_err_condition; /* unit: A */
        uint16_t param_current_err_size;          /* unit: A */
        uint16_t param_prepare_time;              /* unit: period */
        float param_normal_R_range_min;
        float param_normal_R_range_max;
        float param_normal_L_range_min;
        float param_normal_L_range_max;

        uint8_t in_phase_num;
        uint16_t in_debug_info_output;
        uint16_t in_result_output_format;
        uint16_t start;
        uint16_t finished;
        uint16_t failure;
        uint16_t progress;
        float *current_buff;
        float *current_err_buff;
        uint16_t current_index;
        float uin;
        uint16_t uin_index;
        uint16_t is_stable;
        uint16_t prepare_count;

        float tf;    /* the time to reach final value */
        float td;    /* delay time(0.69Tc)：the first time to reach 50% final value */
        float tr;    /* rise time(2.2Tc)：the interval time from 10% final value to 90% final value*/
        float tp;    /* peak time：the first time to exceeds the final value and reaches the maximum value */
        float ts;    /* settling time(3Tc)：the first time to reach the value kept within 5% of final value */
        float sigma; /* overshoot： */

        float final_value;

        float Tc;
        float R;
        float L;

        enum
        {
            SystemId_RL_Progress_PreparationStart = 0,
            SystemId_RL_Progress_PreparationRun,
            SystemId_RL_Progress_UinTestStart,
            SystemId_RL_Progress_UinTest,
            SystemId_RL_Progress_Calculate,
            SystemId_RL_Progress_End,
            SystemId_RL_Progress_Failed,
        } SystemId_RL_Progress_Enum;

        enum
        {
            SystemId_RL_Failure_NoFailure = 0,
            SystemId_RL_Failure_LowPower,
            SystemId_RL_Failure_UnStable,
            SystemId_RL_Failure_ValueAbnormal,
            SystemId_RL_Failure_UnConnected,
        } SystemId_RL_Failure_Enum;
    } system_id_RL;

    /* motor phase check */
    struct MotorPhaseCheck_T
    {
        uint16_t param_run_on_startup;
        float param_motor_resistance_tolerance_standar_percent; /* 0~1.0 */
        uint16_t in_save_into_flash_after_run;
        uint16_t in_debug_info_output;
        uint16_t in_result_output_format;

        uint8_t start;
        uint8_t finished;
        uint8_t failure;
        uint8_t progress;
        float R_A;
        float R_B;
        float L_A;
        float L_B;
        float motor_resistance_tolerance;

        enum
        {
            MotorPhaseCheck_Failure_NoFailure = 0,
            MotorPhaseCheck_Failure_LowPower,
            MotorPhaseCheck_Failure_UnStable,
            MotorPhaseCheck_Failure_ValueAbnormal,
            MotorPhaseCheck_Failure_MotorPhaseNotConnected,
            MotorPhaseCheck_Failure_MotorPhaseResistanceDifferent,
            MotorPhaseCheck_Failure_OtherErr,
        } MotorPhaseCheck_Failure_Enum;
    } motor_phase_check;

    /* zazen mode */
    struct ZazenMode_T
    {
        uint16_t param_en;
        float param_Ts;
        float param_trigger_time_s; /* unit: s */

        struct ZazenMode_Gain_T
        {
            float pos_kp;
            float spd_kp;
            float spd_ki;
            float cur_kp;
            float cur_ki;
        };
        struct ZazenMode_Gain_T param_gain;

        uint32_t trigger_period_count;
        uint32_t time_count;

    } zazen_mode;

    /* data sampler */
    uint16_t data_sampler_nums;
    DataSampler data_sampler[2];

    /* trapezoid curve */
    TrapezoidCurve trapezoid_curve;

    /* rs485 addr */
    uint8_t rs485_addr;
    uint16_t version_print_start;

    /* driver control */
    uint8_t driver_is_enable;

    /* private data */

    /* public method */
    void (*ConfigParam)(struct StepperMotorController *self);
    uint8 (*Init)(struct StepperMotorController *self);
    void (*Config_CmdSystem_Param)(struct StepperMotorController *self);
    void (*ControlLoop)(struct StepperMotorController *self);
    void (*SetControlMode)(struct StepperMotorController *self);
    void (*SetControlAlgorithm)(struct StepperMotorController *self);
    float32 (*Encoder2ElecAngle)(struct StepperMotorController *self);
    float32 (*Encoder2Pos)(struct StepperMotorController *self);
    float32 (*Encoder2PosAbs)(struct StepperMotorController *self);
    float32 (*Encoder2Speed)(struct StepperMotorController *self);
    uint8 (*EncoderCalibration)(struct StepperMotorController *self, uint8_t debug_output);
    void (*UdUqOutput)(struct StepperMotorController *self);
    void (*DriverEnableSolution)(struct StepperMotorController *self);
    void (*StallDetect)(struct StepperMotorController *self);
    void (*ProtectionCheckErr)(struct StepperMotorController *self, uint16 err_num);
    void (*ProtectionCheckWarning)(struct StepperMotorController *self, uint16 warning_num);
    void (*ProtectionDetection)(struct StepperMotorController *self);
    void (*KeyScan)(struct StepperMotorController *self);
    uint8 (*ElecZeroOffsetCalibration)(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format);
    uint8 (*ReceiveDataAnalyse)(struct StepperMotorController *self);
    void (*CommandAnalyse)(struct StepperMotorController *self);
    void (*ClearInitFlag)(struct StepperMotorController *self);
    void (*SystemReboot)(struct StepperMotorController *self);
    void (*TimTask)(struct StepperMotorController *self);
    void (*DriverEnableControl)(struct StepperMotorController *self, uint16_t enable);
    void (*UpdateParam)(struct StepperMotorController *self);
    void (*CheckMotorPhaseOrder)(struct StepperMotorController *self, uint8_t debug_output);
    void (*EncoderCalibrationOfficial_Run)(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format);
    int16_t (*AdcCalibration_Run)(struct StepperMotorController *self);
    uint16_t (*MotorRippleTableCalibrationTask)(struct StepperMotorController *self);
    void (*Cmd_help_Async_Task)(struct StepperMotorController *self);
    void (*Cmd_helpCmd_Async_Task)(struct StepperMotorController *self);
    void (*RegisterCmdToTable)(struct StepperMotorController *self, uint16_t data_num, uint8_t id, uint8_t broadcast_support, char *name, char *description, char *usage, void (*func)(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]));
    void (*SystemIdentification_RL)(struct StepperMotorController *self);
    void (*SystemIdentification_Ke)(struct StepperMotorController *self);
    void (*SystemIdentification_JB)(struct StepperMotorController *self);
    void (*SystemIdentification_Kt)(struct StepperMotorController *self);
    void (*CheckMotorPhaseResistance)(struct StepperMotorController *self);
    void (*VersionPrintTask)(struct StepperMotorController *self);
    void (*ZazenModeRun)(struct StepperMotorController *self);
    void (*ZazenModeEnterOrExit)(struct StepperMotorController *self, uint16_t enter);
    void (*SendFormatedArray)(struct StepperMotorController *self);
    void (*IRQTask_TimeTick)(struct StepperMotorController *self);
    void (*IRQTask_DirCheck)(struct StepperMotorController *self);
    void (*DirProcessing)(struct StepperMotorController *self);
    void (*ParamLoadLogic)(struct StepperMotorController *self);

    /* private method */

} StepperMotorController;

/* Variable Declare ***********************************************************/

/* Inline Function Define *****************************************************/

/* Function Declare ***********************************************************/
uint8 StepperMotorController_Init(struct StepperMotorController *self);
void StepperMotorController_ControlLoop(struct StepperMotorController *self);
void StepperMotorController_SetControlMode(struct StepperMotorController *self);
void StepperMotorController_SetControlAlgorithm(struct StepperMotorController *self);
float32 StepperMotorController_Encoder2ElecAngle(struct StepperMotorController *self);
float32 StepperMotorController_Encoder2Pos(struct StepperMotorController *self);
float32 StepperMotorController_Encoder2PosAbs(struct StepperMotorController *self);
float32 StepperMotorController_Encoder2Speed(struct StepperMotorController *self);
uint8 StepperMotorController_EncoderCalibration(struct StepperMotorController *self, uint8_t debug_output);
void StepperMotorController_UdUqOutput(struct StepperMotorController *self);
void StepperMotorController_DriverEnableSolution(struct StepperMotorController *self);
void StepperMotorController_StallDetect(struct StepperMotorController *self);
void StepperMotorController_ProtectionCheckErr(struct StepperMotorController *self, uint16 err_num);
void StepperMotorController_ProtectionCheckWarning(struct StepperMotorController *self, uint16 warning_num);
void StepperMotorController_ProtectionDetection(struct StepperMotorController *self);
void StepperMotorController_KeyScan(struct StepperMotorController *self);
uint8 StepperMotorController_ElecZeroOffsetCalibration(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format);
uint8 StepperMotorController_ReceiveDataAnalyse(struct StepperMotorController *self);
void StepperMotorController_CommandAnalyse(struct StepperMotorController *self);
void StepperMotorController_ClearInitFlag(struct StepperMotorController *self);
void StepperMotorController_SystemReboot(struct StepperMotorController *self);
void StepperMotorController_TimTask(struct StepperMotorController *self);
void StepperMotorController_DriverEnableControl(struct StepperMotorController *self, uint16_t enable);
void StepperMotorController_UpdateParam(struct StepperMotorController *self);
void StepperMotorController_CheckMotorPhaseOrder(struct StepperMotorController *self, uint8_t debug_output);
void StepperMotorController_EncoderCalibrationOfficial_Run(struct StepperMotorController *self, uint8_t debug_output, uint8_t output_format);
int16_t StepperMotorController_AdcCalibration_Run(struct StepperMotorController *self);
uint16_t StepperMotorController_MotorRippleTableCalibrationTask(struct StepperMotorController *self);
void StepperMotorController_Cmd_help_Async_Task(struct StepperMotorController *self);
void StepperMotorController_Cmd_helpCmd_Async_Task(struct StepperMotorController *self);
void StepperMotorController_RegisterCmdToTable(struct StepperMotorController *self, uint16_t data_num, uint8_t id, uint8_t broadcast_support, char *name, char *description, char *usage, void (*func)(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]));
void StepperMotorController_SystemIdentification_RL(struct StepperMotorController *self);
void StepperMotorController_SystemIdentification_Ke(struct StepperMotorController *self);
void StepperMotorController_SystemIdentification_JB(struct StepperMotorController *self);
void StepperMotorController_SystemIdentification_Kt(struct StepperMotorController *self);
void StepperMotorController_CheckMotorPhaseResistance(struct StepperMotorController *self);
void StepperMotorController_VersionPrintTask(struct StepperMotorController *self);
void StepperMotorController_ZazenModeRun(struct StepperMotorController *self);
void StepperMotorController_ZazenModeEnterOrExit(struct StepperMotorController *self, uint16_t enter);
void StepperMotorController_SendFormatedArray(struct StepperMotorController *self);
void StepperMotorController_IRQTask_TimeTick(struct StepperMotorController *self);
void StepperMotorController_IRQTask_DirCheck(struct StepperMotorController *self);
void StepperMotorController_DirProcessing(struct StepperMotorController *self);
void StepperMotorController_ParamLoadLogic(struct StepperMotorController *self);

void CmdFunction_help(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_reboot(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_encoder_calibrate(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_encoder_calibrate_official(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_elec_offset_calibrate(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_control(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_sys_param(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_flash_param(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_get(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_logger(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_ripple_cal(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_boot(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_protection(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_systemid(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_rs485_addr(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_version(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_control_loop(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_emergency_pin_mode(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_data_sampler(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
void CmdFunction_trajectory_plan(void *smc, char cmd_and_arg[COMMAND_MAX_NUM][COMMAND_MAX_STRLEN]);
#endif
